#!/usr/bin/python
# -*- coding: UTF-8 -*-

from time import sleep
from MiniBotControlLib import MiniBotControlLib
from BwSensorLib import BwSensorLib

# 声明控制库对象
mlib = MiniBotControlLib("udp")
# 声明传感器库对象
sensorlib = BwSensorLib(mlib)
# 连接机器人
ip = "10.10.100.254"
mlib.Connect(ip, 8899)

# 识别颜色点亮对应颜色的RGB灯
def main():
    # RGB灯颜色 红色：[255, 0, 0]，绿色：[0, 255, 0]，蓝色[0, 0, 255]， 熄灭：[0, 0, 0]
   rgbValue = [[255,0,0],[0,255,0],[0,0,255],[0,0,0]]
   while 1 :
      color = sensorlib.readColorSensor() # 读取颜色传感器数据
      sleep(0.2)
   # 检测到红色
      if(color == 1):  
         sensorlib.setRGBLightSensor(rgbValue[0]) # 亮红灯
   # 检测到绿色
      elif(color == 2):
         sensorlib.setRGBLightSensor(rgbValue[1]) # 亮绿灯
   # 检测到蓝色
      elif(color == 3):
         sensorlib.setRGBLightSensor(rgbValue[2]) # 亮蓝灯
   # 检测没有红色
      else:
         sensorlib.setRGBLightSensor(rgbValue[3]) # 灭灯

if __name__ == "__main__":
    try:
        main()
    except KeyboardInterrupt:
        print(" Program closed! ")
    pass
